import cv2
import numpy as np
import color_matrix
import color_matrix_js
import Find_ring
import yaw_detect
import serial
import socket
import pyzbar.pyzbar as pyzbar
import time

import time


'''HSV格式  H色调0-180度  S饱和度0-255  V明度0-255'''
'''红色范围'''
lower_red = np.array([160, 30, 30])
upper_red = np.array([180, 255, 255])
lower_red1 = np.array([0, 30, 30])
upper_red1 = np.array([10, 255, 255])
'''绿色范围'''
lower_green = np.array([45, 70, 70])
upper_green = np.array([90, 255, 255])
'''蓝色范围'''
lower_blue = np.array([100, 70, 70])
upper_blue = np.array([130, 255, 255])


'''640 x 480'''
mtx = np.asarray([[659.58455902, 0, 359.17778896], [0, 660.46047903, 281.17166208], [0, 0, 1]])
dist = np.asarray([[-4.83472356e-01, 4.00184597e-01, -1.48393000e-03, 1.72148648e-04, -2.81260521e-01]])
newcameramtx = np.asarray([[655.20635986, 0, 356.79363971], [0, 632.92816162, 295.12993806], [0, 0, 1]])

'''圆环摄像头640 480'''
mtx_3 = np.asarray([[674.13564504, 0, 310.30167109], [0, 678.06752334, 245.37958894], [0, 0, 1]])
dist_3 = np.asarray([[-0.47084501, 0.13509371, -0.00962877, 0.00522067, 0.13195138]])
newcameramtx_3 = np.asarray([[610.30535889, 0, 321.15842895], [0, 566.75701904, 217.02414996], [0, 0, 1]])

"""摄像头标定参数"""
mapx_1, mapy_1 = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (640, 480), 5)  # 640x480 摄像头标定参数
mapx_3, mapy_3 = cv2.initUndistortRectifyMap(mtx_3, dist_3, None, newcameramtx_3, (800, 600), 5)  # 800600 摄像头标定参数

"""---------------------初始化参数-------------------------"""
yaw = yaw_detect.yaw()
yaw.image_init(5, 20, 60)
yaw.parameter_init(Threshold=330)
################################

find_color1 = color_matrix_js.color_matrix()
find_color1.init_threshold(lower_red, upper_red, lower_red1, upper_red1, lower_green, upper_green, lower_blue,
                          upper_blue)
find_color1.Program_parameters_init(600, 10, 0.05, 5, 20)  # 色块面积，色块旋转角度，连线最大K值，点到线最大距离，X均匀度

################################
find_color = color_matrix.color_matrix()
find_color.init_threshold(lower_red, upper_red, lower_red1, upper_red1, lower_green, upper_green, lower_blue,
                          upper_blue)
find_color.Program_parameters_init(500, 20, 0.20, 10, 50, 50)  # 色块面积，色块旋转角度，连线最大K值，点到线最大距离，X均匀度，Y均匀度
find_color.Graphics_parameters_init(5, 11)  # 二值化之后的腐蚀度，提高腐蚀度可以消除噪点和分离过近的两个色块边界    模糊度

find_ring = Find_ring.ring()
find_ring.image_init((5, 5), 5)  # 高斯模糊参数，中值滤波参数，canny_min , canny_max ，图像膨胀参数
find_ring.parameter_init(17000, 20, 100, 50, 200, 100, 150,
                         debug=True)  # 寻找物块的面积阈值，param1（canny的最大值，最小值为最大值的一半），param2（阈值），minRadius
# （最小半径），maxRadius（最大半径）
# 色环间距（这个值越大判断色环为两个圆需要的距离越大），色环中心到物块中心最大间距
portX = '/dev/ttyUSB0'
bps = 115200
timex = 0.005
ser = serial.Serial(portX, bps, timeout=timex)

"""---------------------------------------------------------"""



X = np.asarray([0])  # 最优估计状态
P = np.asarray([1])  # 最优状态协方差矩阵
Q = np.asarray([0.005])  # 状态转移方差  (预测模型的方差)
F = np.asarray([1])  # 状态转移矩阵
H = np.asarray([1])  # 观测矩阵
R = 0.1  # 观测噪声方差
B = np.asarray([1])  # 控制矩阵
EYE = np.asarray([1])

def get_host_ip():
    """
    查询本机ip地址
    :return:
    """
    try:
        s=socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
        s.connect(('8.8.8.8',80))
        ip=s.getsockname()[0]
    finally:
        s.close()

    return ip

# 旋转angle角度，缺失背景白色（255, 255, 255）填充
def rotate_bound_white_bg(image, angle):
    # grab the dimensions of the image and then determine the
    # center
    (h, w) = image.shape[:2]
    (cX, cY) = (w // 2, h // 2)

    # grab the rotation matrix (applying the negative of the
    # angle to rotate clockwise), then grab the sine and cosine
    # (i.e., the rotation components of the matrix)
    # -angle位置参数为角度参数负值表示顺时针旋转; 1.0位置参数scale是调整尺寸比例（图像缩放参数），建议0.75
    M = cv2.getRotationMatrix2D((cX, cY), -angle, 1.0)
    cos = np.abs(M[0, 0])
    sin = np.abs(M[0, 1])

    # compute the new bounding dimensions of the image
    nW = int((h * sin) + (w * cos))
    nH = int((h * cos) + (w * sin))

    # adjust the rotation matrix to take into account translation
    M[0, 2] += (nW / 2) - cX
    M[1, 2] += (nH / 2) - cY

    # perform the actual rotation and return the image
    # borderValue 缺失背景填充色彩，此处为白色，可自定义
    return cv2.warpAffine(image, M, (nW, nH), borderValue=(255, 255, 255))
    # borderValue 缺省，默认是黑色（0, 0 , 0）
    # return cv2.warpAffine(image, M, (nW, nH))


def Kalman_filter(val):
    global X, P, Q, F, H, R, B, EYE
    X_ = F * X
    P_ = F * P * F.T + Q
    K = (P_ * H.T) / (H * P_ * H.T + R)
    X = X_ + K * (val - H * X_)
    P = (EYE - K * H) * P_
    return X

    

last_barcode = []
my_barcode = []
number_str = ['1','2','3']


class main_obj(object):

    def __init__(self):
        self.down = None
        self.front = None
    
    def ready_front(self):
        time.sleep(0.2)
        while True:
            self.front = cv2.VideoCapture('/dev/video0')  # 调用摄像头1   
            if self.front.isOpened():
                print('front,is open')
                break
            else:
                time.sleep(0.5)
                print('front,not open,try again')
        self.front.set(cv2.CAP_PROP_FRAME_WIDTH, 640)  # 设置图像宽度
        self.front.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)  # 设置图像高度
        self.front.set(cv2.CAP_PROP_FOURCC,cv2.VideoWriter_fourcc('M','J','P','G'))
        self.front.set(cv2.CAP_PROP_BUFFERSIZE,1)
        
    def ready_down(self):
        time.sleep(0.2)
        while True:
            self.down = cv2.VideoCapture('/dev/video2')
            if self.down.isOpened():
                print('down,is open')
                break
            else:
                time.sleep(0.5)
                print('down,not open,try again')
        self.down.set(cv2.CAP_PROP_FRAME_WIDTH, 640)  # 设置图像宽度
        self.down.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)  # 设置图像高度
        self.down.set(cv2.CAP_PROP_FOURCC,cv2.VideoWriter_fourcc('M','J','P','G'))
        self.down.set(cv2.CAP_PROP_BUFFERSIZE,1)
        
    
    def release_front(self):
        try:
            self.front.release()
        except Exception as e:
            print('front_rel_err')
    def release_down(self):
        try:
            self.down.release()
        except Exception as e:
            print('down_rel_err:')
    
    def get_front(self):
        frame_bit, frame = self.front.read()
        if(frame_bit):
            frame = rotate_bound_white_bg(frame,1.0)
            return frame_bit, frame
        else:
            return frame_bit, None
    def get_down(self):
        frame_bit, frame = self.down.read()
        if(frame_bit):
            frame = rotate_bound_white_bg(frame,-0.75)
            return frame_bit, frame
        else:
            return frame_bit, None




"""当前状态等于状态0"""
status = 0
cam_status = 0    #0None 1front 2down
DEBUG = True#False
"""----------------"""
if __name__ == '__main__':
    white = cv2.imread('white.jpg')
    cv2.imshow('white', white)
    cam_obj = main_obj()
        
    a = 0
    while True:
        try:
            data = ser.readline()
            if data == b'ZT1\r\n':  # 准备检测二维码
                status = 1
            elif data == b'ZT2\r\n':  # 检测颜色
                status = 2
            elif data == b'ZT3\r\n':  # 检测圆环
                status = 3
            elif data == b'ZT4\r\n':  # 读取条形码
                status = 4
            elif data == b'ZT5\r\n':  # 检测条形码位置
                status = 5
            elif data == b'ZT6\r\n':  # 决赛颜色识别
                status = 6
            elif data == b'ZT0\r\n':  # yaw
                status = 0
            elif data == b'ZT7\r\n':  # yaw
                status = 7
            else:
                pass
            
            if len(data) != 0:
                print(data,status)
        except:
            pass
        if status == 7:
            while True:
                my_barcode = []
                udp_socket=socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
                udp_socket.bind(("",8080))
                data_info, ip_data = udp_socket.recvfrom(1024)
                barcodeData = data_info.decode("utf-8")
                if barcodeData:
                    for i in barcodeData:
                        if i in number_str:
                            my_barcode.append(i)
                            if len(my_barcode)==3:
                                my_barcode.append('+')
                    if len(my_barcode)==7:
                        print("".join(my_barcode))
                        ser.write(('E:' + str("".join(my_barcode)) + '\r\n').encode("GBK"))
                        ser.write(('E:' + str("".join(my_barcode)) + '\r\n').encode("GBK"))
                        ser.write(('E:' + str("".join(my_barcode)) + '\r\n').encode("GBK"))
                        cv2.putText(white, str("".join(my_barcode)), (25, 210), cv2.FONT_HERSHEY_SIMPLEX, 5, (0, 0, 255), 15)
                        cv2.imshow('white', white)
                        status = 6
                        break

        if status == -2:  # 获取角度 通过COM口发送给stm32 并且等待stm32发出颜色识别指令，之后进入status 1
            if cam_status != 2:
                cam_status = 2
                cam_obj.release_front()
                cam_obj.ready_down()
            ret_yaw, frame_yaw = cam_obj.get_down()
            if ret_yaw:
                cv2.imshow('yaw', frame_yaw)
            cv2.waitKey(20)
            a = a + 1
            if a == 20:
                a = 0
                status = -1
        if status == -1:  # 获取角度 通过COM口发送给stm32 并且等待stm32发出颜色识别指令，之后进入status 1
            if cam_status != 1:
                cam_status = 1
                cam_obj.release_down()
                cam_obj.ready_front()
            ret_color, frame_color = cam_obj.get_front()
            if ret_color:
                cv2.imshow('yaw1', frame_color)
            cv2.waitKey(20)
            a = a + 1
            if a == 20:
                a = 0
                cv2.destroyWindow('yaw1')
                cv2.destroyWindow('yaw')
                status = 0
        if status == 0:  # 获取角度 通过COM口发送给stm32 并且等待stm32发出颜色识别指令，之后进入status 1
            if cam_status != 2:
                cam_status = 2
                cam_obj.release_front()
                cam_obj.ready_down()
            ret_yaw, frame_yaw = cam_obj.get_down()
            if ret_yaw:
                frame_yaw = frame_yaw[50:330,20:640]
                angle, frame1 = yaw.get(frame_yaw)
                if angle:
                    angle = Kalman_filter(angle)
                    angle1 = round(angle[0], 2)
                    #print(angle1)
                    ser.write(('A:' + str(angle1) + '\r\n').encode("GBK"))
                if DEBUG:
                    cv2.imshow('yaw', frame1)
                cv2.waitKey(2)

            
        if status == 1:  # 等待检测二维码
            if cam_status != 1:
                cam_status = 1
                cam_obj.release_down()
                cam_obj.ready_front()
            ret_color, frame_color = cam_obj.get_front()
            if ret_color:
                #frame_color = rotate_bound_white_bg(frame_color, -1.6)
                #frame_color = frame_color[180:900, 320:1600]
                gray = cv2.cvtColor(frame_color, cv2.COLOR_BGR2GRAY)
                code_erwei_area = gray
                barcodes1 = pyzbar.decode(code_erwei_area)
                for barcode in barcodes1:
                    (x, y, w, h) = barcode.rect
                    cv2.rectangle(frame_color, (x , y ),
                                  (x + w, y + h),
                                  (255, 255, 0), 2)
                    barcodeData = barcode.data.decode("utf-8")
                    barcodeType = barcode.type
                    if barcodeData:
                        for i in barcodeData:
                            if i in number_str:
                                my_barcode.append(i)
                                if len(my_barcode)==3:
                                    my_barcode.append('+')
                                if len(my_barcode)==7:
                                    break
                        print("".join(my_barcode))
                        ser.write(('E:' + str("".join(my_barcode)) + '\r\n').encode("GBK"))
                        ser.write(('E:' + str("".join(my_barcode)) + '\r\n').encode("GBK"))
                        ser.write(('E:' + str("".join(my_barcode)) + '\r\n').encode("GBK"))
                        cv2.putText(white, str("".join(my_barcode)), (25, 210), cv2.FONT_HERSHEY_SIMPLEX, 5, (0, 0, 255), 15)
                        my_barcode = []
                        cv2.imshow('white', white)
                        status = 2
                        if DEBUG:
                            cv2.destroyWindow('color')

                if DEBUG and status == 1:
                    cv2.imshow('color', frame_color)
                cv2.waitKey(2)

        if status == 2:
            if cam_status != 1:
                cam_status = 1
                cam_obj.release_down()
                cam_obj.ready_front()
            #cv2.waitKey(0)
            #ret_color, frame_color = cam_obj.get_front()
            #ret_color, frame_color = cam_obj.get_front()
            ret_color, frame_color = cam_obj.get_front()
            if ret_color:
                #cv2.imwrite('123.jpg',frame_color)
                #frame_color = frame_color[180:900, 320:1600]
                frame1 = find_color.find1(frame_color)
                frame2,color_black,color = find_color.find(frame1)
                if DEBUG:
                    cv2.imshow('color', frame2)
                if color and len(set(color[0:3])) == 3 and len(set(color[4:7])) == 3:
                    print(color)
                    ser.write(('C:' + str(color) + '\r\n').encode("GBK"))
                    ser.write(('C:' + str(color) + '\r\n').encode("GBK"))
                    ser.write(('C:' + str(color) + '\r\n').encode("GBK"))
                    status = 0
                    if DEBUG:
                        cv2.destroyWindow("color")
        cv2.waitKey(2)
        if status == 3:
            if cam_status != 2:
                cam_status = 2
                cam_obj.release_front()
                cam_obj.ready_down()
            ret_ring, frame_ring = cam_obj.get_down()
            if ret_ring:
                frame_ring = frame_ring[0:480,100:540]
                frame_ring_2, x, y = find_ring.find(frame_ring)
                if x:
                    x = int(x)
                    y = int(y)
                    print(x,y)
                    ser.write(('R:' + str(x) + ',' + str(y) + '\r\n').encode("GBK"))
                if DEBUG:
                    cv2.imshow('ring', frame_ring_2)
                    
            cv2.waitKey(2)
            if status == 0:
                if DEBUG:
                    cv2.destroyWindow("ring")
            
        if status == 4:  # 检测条形码内容
            if cam_status != 1:
                cam_status = 1
                cam_obj.release_down()
                cam_obj.ready_front()
            ret_color, frame_color = cam_obj.get_front()
            if ret_color:
                #frame_color = rotate_bound_white_bg(frame_color, -1.6)
                #frame_color = frame_color[180:900, 320:1600]
                gray = cv2.cvtColor(frame_color, cv2.COLOR_BGR2GRAY)
                barcodes = pyzbar.decode(gray)
                for barcode in barcodes:
                    barcodeData = barcode.data.decode("utf-8")
                    if last_barcode != barcodeData and (barcodeData == '1' or barcodeData == '2' or barcodeData == '3'):
                        last_barcode = barcodeData
                        print(barcodeData)
                        ser.write(('T:' + str(barcodeData[0]) + '\r\n').encode("GBK"))
                if DEBUG:
                    cv2.imshow('color', frame_color)
            if status != 4:
                cv2.destroyWindow("color")
            cv2.waitKey(2)

        if status == 5:  # 检测条形码位置
            if cam_status != 1:
                cam_status = 1
                cam_obj.release_down()
                cam_obj.ready_front()
            ret_color, frame_color = cam_obj.get_front()
            if ret_color:
                #frame_color = rotate_bound_white_bg(frame_color, -1.6)
                #frame_color = frame_color[180:900, 320:1600]
                gray = cv2.cvtColor(frame_color, cv2.COLOR_BGR2GRAY)
                # 黑帽
                kernel = np.ones((1, 3), np.uint8)
                image = cv2.morphologyEx(gray, cv2.MORPH_BLACKHAT, kernel, anchor=(1, 0))
                # 阈值处理
                thresh, image = cv2.threshold(image, 10, 255, cv2.THRESH_BINARY)
                # operazioni  morfologiche 形态运算
                kernel = np.ones((3, 30), np.uint8)
                image = cv2.morphologyEx(image, cv2.MORPH_DILATE, kernel, anchor=(-1, -1),
                                         iterations=2)  # dilatazione膨胀求局部最大值,腐蚀求局部最小值
                image = cv2.morphologyEx(image, cv2.MORPH_CLOSE, kernel, anchor=(-1, -1), iterations=2)  # chiusura关闭
                kernel = np.ones((55, 55), np.uint8)
                image = cv2.morphologyEx(image, cv2.MORPH_OPEN, kernel,
                                         iterations=1)  # 最后的预处理步骤是应用具有很大内核的开运算符，以删除太少而无法适合条形码形状的元素。
                cv2.imshow('1',image)
                if DEBUG:
                    #cv2.imshow("last_demo", image)
                    pass

                contours, hierarchy = cv2.findContours(image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
                for c in range(len(contours)):
                    rect = cv2.minAreaRect(contours[c])  # 获取最小外接矩形的中心（x，y），（宽度，高度），旋转角度
                    if cv2.contourArea(contours[c]) > 10000:
                        cx, cy = rect[0]  # 中心坐标
                        box = cv2.boxPoints(rect)  # 返回矩形顶点坐标[ [x0,y0], [x1,y1], [x2,y2], [x3,y3] ]
                        box = np.int0(box)
                        cv2.drawContours(frame_color, [box], 0, (0, 255, 0), 2)
                        cv2.circle(frame_color, (np.int32(cx), np.int32(cy)), 2, (255, 255, 255), 2, 8, 0)
                        #print(int(cx))
                        ser.write(('W:' + str(int(cx)) + '\r\n').encode("GBK"))
                if DEBUG:
                    cv2.imshow('color', frame_color)
            if status != 5:
                cv2.destroyWindow("color")
            cv2.waitKey(2)
        if status == 6:#决赛半成品区域颜色识别
            if cam_status != 1:
                cam_status = 1
                cam_obj.release_down()
                cam_obj.ready_front()
            ret_color, frame_color = cam_obj.get_front()
            if ret_color:
                num = 0
                frame_copy = frame_color.copy()[337:429, 0:657]
                find_color1.Graphics_parameters_init(5, 11, 63, 20, 163, 400, 130)  # 二值化之后的腐蚀度（提高腐蚀度可以消除噪点和分离过近的两个色块边界）， 模糊度
                                                                                    # 左边黑块宽度，右边黑块宽度,左边黑块，右边黑块，黑块宽度
                frame1, color_block, color1 = find_color1.find(frame_copy)
                if color1:
                    num = num + 1

                frame_copy1 = frame_color.copy()[246:304, 0:657]
                find_color1.Graphics_parameters_init(5, 11, 141, 90, 220, 390, 90)  # 二值化之后的腐蚀度（提高腐蚀度可以消除噪点和分离过近的两个色块边界）， 模糊度
                frame2, color_block, color2 = find_color1.find(frame_copy1)               # 左边黑块宽度，右边黑块宽度,左边黑块，右边黑块，黑块宽度
                if color2:
                    num = num + 1
                if DEBUG:
                    cv2.imshow('c1',frame1)
                    cv2.imshow('c2',frame2)
                if num == 2 and len(set(color1)) == 3 and len(set(color2)) == 3:
                    print(color1+color2)
                    ser.write(('C:' + str(color1) + str(color2) + '\r\n').encode("GBK"))
                    ser.write(('C:' + str(color1) + str(color2) + '\r\n').encode("GBK"))
                    ser.write(('C:' + str(color1) + str(color2) + '\r\n').encode("GBK"))
                    status = 0
                    if DEBUG:
                        cv2.destroyWindow("c1")
                        cv2.destroyWindow("c2")
            cv2.waitKey(5)
